<template>
  <el-card ref="card">
    <template #header>
      <el-row class="mapView">
        <el-col :span="6">
          <el-space wrap>
            <el-button type="primary" @click="toggleTeleopMode">摇杆</el-button>
            <el-button v-if="!teleopJoystick" type="primary" @click="forceStop"
              >急停</el-button
            >
            <el-button type="primary" @click="refreshMap">连接</el-button>
            <el-button type="primary" :icon="Rank" @click="moveMap"
              >移动</el-button
            >
            <el-button type="primary" :icon="Position" @click="toggleCoordinate"
              >坐标</el-button
            >
            <el-button type="primary" @click="dialogVisible = true">
              选择地图
            </el-button>
            <!-- <el-button plain @click="dialogVisible = true">
              编辑地图
            </el-button> -->
          </el-space>
        </el-col>
        <span class="slider-demo-block">缩放：</span>
        <el-col :span="5">
          <el-slider
            v-model="zoomLevel"
            show-input
            :min="0.1"
            :max="5"
            :step="0.1"
            @input="handleSliderChange"
          />
        </el-col>
        <el-button type="warning" :icon="Refresh" @click="resetMap"
          >重置</el-button
        >
        <el-col :span="1" />
        <el-col :span="6">
          <el-button type="warning" @click="navigation">导航</el-button>
          <el-button type="warning" @click="intialPose">起点</el-button>
          <el-button type="warning" @click="goalPose">终点</el-button>
          <el-button :type="multiGoalType" @click="multiGoal"
            >多目标点</el-button
          >
          <el-button type="danger" :icon="Close" @click="cleanNodes">
            关闭
          </el-button>
        </el-col>
      </el-row>
    </template>
    <div class="map-container">
      <span />
      <el-text class="mx-1">坐标 x: {{ coor_x }}, y: {{ coor_y }}</el-text>
      <div id="map" />
    </div>

    <el-dialog v-model="dialogVisible" title="选择地图" :width="300">
      <el-input
        v-model="mapName"
        placeholder="请输入地图名"
        style="margin-bottom: 20px"
      />
      <template v-slot:footer>
        <div class="dialog-footer" style="text-align: right">
          <el-button size="small" @click="dialogVisible = false"
            >取 消</el-button
          >
          <el-button type="danger" size="small" @click="openMap"
            >确 定</el-button
          >
        </div>
      </template>
    </el-dialog>

    <div class="virtual-gamepad-teleop" />
  </el-card>
</template>

<script setup lang="ts">
const ROSLIB = window.ROSLIB;
const ROS2D = window.ROS2D;
const NAV2D = window.NAV2D;
import nipplejs from "nipplejs";
import { onMounted, ref, reactive, watch } from "vue";
import ROSCommand from "@/api/user";
import { ElMessage } from "element-plus";
import { ip } from "@/api/utils";
import { Refresh, Rank, Position, Close } from "@element-plus/icons-vue";

const onMapping = ref(false);
const dialogVisible = ref(false);
const card = ref(null);
const coor_x = ref(0);
const coor_y = ref(0);
// ROS Related
let ros2 = undefined;
let map_data = undefined;
const globalState = reactive({
  height: 0,
  width: 0,
  zoomscale: 1,
  init_pose: false,
  goal_pose: false,
  isNav: false,
  flag: "line",
  color: "#000000",
  lineWidth: 5
});
let isMoving = false;
let isCoor = false;
let isPanning = false;
const zoomLevel = ref(1);
let viewer = undefined;
let zoomView = undefined;
let panView = undefined;
let initialPose = null;
let navigator = null;

const mulitiGoalMaker = new ROS2D.NavigationPoint({
  size: 1.0,
  strokeSize: 0.3,
  strokeColor: createjs.Graphics.getRGB(0x023678, 0.8),
  fillColor: createjs.Graphics.getRGB(0x023678, 0.8)
});
const mapName = ref("map");
const rosCommandInstance = new ROSCommand();

const initializeROSCommunication = () => {
  rosCommandInstance.Command(
    "roslaunch",
    "rosbridge_server rosbridge_websocket.launch"
  );
  rosCommandInstance.Command(
    "rosrun",
    "tf2_web_republisher tf2_web_republisher"
  );
};
const handleSliderChange = value => {
  globalState.zoomscale = value;
};

const cleanNodes = () => {
  ros2.getNodes(nodes => {
    console.log("Active nodes:");
    nodes.forEach(node => {
      console.log(node);
      if (node !== "rosbridge_websocket" && node !== "tf2_web_republisher") {
        rosCommandInstance.Command("rosnode", `kill ${node}`);
      }
    });
  });
};

const openMap = () => {
  rosCommandInstance.Command(
    "rosrun",
    `map_server map_server ~/map/${mapName.value}.yaml`
  );
  dialogVisible.value = false;
};

const resetMap = () => {
  zoomView.startZoom(
    card.value.$el.offsetWidth / 2,
    card.value.$el.offsetHeight / 2
  );
  console.log("zoom:", globalState.zoomscale);
  zoomView.zoom(1);
  globalState.zoomscale = 1;
  zoomLevel.value = 1;
};

const navigation = () => {
  rosCommandInstance.Command(
    "python3",
    "/home/cs/test_ws/src/race_navigation/scripts/startNav.py"
  );
  // rosCommandInstance.Command("rosrun", "rviz rviz -d /home/cs/test_ws/src/ifly_navigation/rviz/ifly_navigation_dwa_rviz.rviz");
};

const moveMap = () => {
  isMoving = true;
  isCoor = false;
  if (navigator) {
    navigator.destroy();
    navigator = null;
  }
  if (initialPose) {
    initialPose.destroy();
    initialPose = null;
  }
  viewer.scene.on("stagemousedown", function (event) {
    if (isMoving) {
      isPanning = true;
      panView.startPan(event.stageX, event.stageY);
      zoomView.startZoom(event.stageX, event.stageY);
    }
  });

  viewer.scene.on("stagemousemove", function (event) {
    if (isMoving && isPanning) {
      panView.pan(event.stageX, event.stageY);
    }
  });

  viewer.scene.on("stagemouseup", function () {
    if (isMoving) {
      isPanning = false;
    }
  });
};

const coordinate = ref(false);

const toggleCoordinate = () => {
  coordinate.value = !coordinate.value;
  if (coordinate.value) {
    startCoordinate();
  } else {
    stopCoordinate();
  }
};

let positionsArray = [];
let isMultiGoal = false;
const startCoordinate = () => {
  isCoor = true;
  isMoving = false;
  viewer.scene.on("stagemousemove", function (event) {
    if (isCoor) {
      const position = viewer.scene.globalToRos(event.stageX, event.stageY);
      const positionVec3 = new ROSLIB.Vector3(position);
      coor_x.value = positionVec3.x.toFixed(3);
      coor_y.value = positionVec3.y.toFixed(3);
    }
  });
  if (isMultiGoal) {
    viewer.scene.on("stagemousedown", function (event) {
      if (isCoor) {
        const position = viewer.scene.globalToRos(event.stageX, event.stageY);
        const positionVec3 = new ROSLIB.Vector3(position);
        positionsArray.push({ x: positionVec3.x, y: positionVec3.y }); // 将位置信息添加到数组中
        mulitiGoalMaker.x = positionVec3.x;
        mulitiGoalMaker.y = -positionVec3.y;
        mulitiGoalMaker.rotation = 0;
        viewer.scene.addChild(mulitiGoalMaker);
      }
    });
    console.log(positionsArray);
  }
};

const stopCoordinate = () => {
  isCoor = false;
};

const intialPose = () => {
  isMoving = false;
  isCoor = false;
  if (navigator) {
    navigator.destroy();
    navigator = null;
  }
  if (!initialPose) {
    initialPose = new NAV2D.InitialPose({
      ros: ros2,
      rootObject: viewer.scene
    });
  }
};

const goalPose = () => {
  isMoving = false;
  isCoor = false;
  if (initialPose) {
    initialPose.destroy();
    initialPose = null;
  }
  if (!navigator) {
    navigator = new NAV2D.Navigator({
      ros: ros2,
      rootObject: viewer.scene,
      withOrientation: true
    });
  }
};

const multiGoalType = ref("warning");
const multiGoal = () => {
  if (isMultiGoal) {
    isMultiGoal = false;
    multiGoalType.value = "warning";
    if (positionsArray.length > 0) {
      navigator = new NAV2D.Navigator({
        ros: ros2,
        rootObject: viewer.scene,
        withOrientation: false,
        positions: positionsArray
      });
    }
    positionsArray = [];
  } else {
    isMoving = false;
    isCoor = false;
    isMultiGoal = true;
    multiGoalType.value = "danger";
    if (navigator) {
      navigator.destroy();
      navigator = null;
    }
    if (initialPose) {
      initialPose.destroy();
      initialPose = null;
    }
  }
};
onMounted(() => {
  globalState.width = card.value.$el.offsetWidth;
  globalState.height = card.value.$el.offsetHeight;
  ros2 = new ROSLIB.Ros({
    url: "ws://" + ip("9090")
  });
  ros2.on("error", function (error) {
    console.error("Failed to connect to ROS: ", error);
    ElMessage.error("ROS连接失败!");
  });
  map_data = new ROSLIB.Topic({
    ros: ros2,
    name: "/map",
    messageType: "nav_msgs/msg/OccupancyGrid"
  });
  viewer = new ROS2D.Viewer({
    divID: "map",
    width: globalState.width,
    height: globalState.height
  });
  zoomView = new ROS2D.ZoomView({
    rootObject: viewer.scene
  });
  panView = new ROS2D.PanView({ rootObject: viewer.scene });
  map_data.subscribe(map => {
    if (
      globalState.height != map.info.height ||
      globalState.width != map.info.width
    ) {
      globalState.height = map.info.height;
      globalState.width = map.info.width;
      viewer.width = globalState.width;
      viewer.height = globalState.height;
      zoomView.startZoom(
        card.value.$el.offsetWidth / 2,
        card.value.$el.offsetHeight / 2
      );
    }
  });

  watch(
    () => [globalState.zoomscale],
    ([newScale]) => {
      zoomView.zoom(newScale);
    }
  );

  new NAV2D.OccupancyGridClientNav({
    ros: ros2,
    serverName: "/move_base",
    continuous: true,
    rootObject: viewer.scene,
    viewer: viewer,
    withOrientation: true
  });
});

// Joystick Related
const teleopJoystick = ref(true);
let teleopPublisher = undefined;
let teleopGamepad = undefined;
const toggleTeleopMode = () => {
  if (teleopJoystick.value) {
    teleopGamepad = nipplejs.create({
      zone: document.querySelector(".virtual-gamepad-teleop"),
      mode: "static",
      position: { left: "85%", top: "85%" },
      color: "#0072FF",
      threshold: 0.0,
      fadeTime: 400,
      maxNumberOfNipples: 1
    });

    teleopPublisher = new ROSLIB.Topic({
      ros: ros2,
      name: "/cmd_vel",
      messageType: "geometry_msgs/msg/Twist"
    });

    teleopGamepad.on("move", function (event, data) {
      const r = data.distance / 200;
      const theta = data.angle.radian;
      const cmdVelData = new ROSLIB.Message({
        linear: {
          x: r * Math.sin(theta)
        },
        angular: {
          z: -r * Math.cos(theta)
        }
      });

      teleopPublisher.publish(cmdVelData);
    });
  } else {
    if (teleopGamepad != undefined) {
      const cmdVelData = new ROSLIB.Message({
        linear: {
          x: 0
        },
        angular: {
          z: 0
        }
      });
      teleopPublisher.publish(cmdVelData);
      teleopGamepad.destroy();
    }
  }
  teleopJoystick.value = !teleopJoystick.value;
};

// Force Stop Related
const forceStop = () => {
  const cmdVelData = new ROSLIB.Message({
    linear: {
      x: 0
    },
    angular: {
      z: 0
    }
  });
  teleopPublisher.publish(cmdVelData);
};

const refreshMap = () => {
  initializeROSCommunication();
  onMapping.value = true;
};
</script>

<style lang="scss" scoped>
.flex_r_b {
  display: flex;
  justify-content: space-between;
  align-items: center;
}

.mx-1 {
  position: absolute;
  right: 0;
  top: 0;
  width: auto;
  height: auto;
  border: 3px solid var(--el-border-color);
}

.map-container {
  width: 70vw;
  height: 75vh;
  position: relative;
}

.el-dialog__wrapper[fullscreen="true"] .el-dialog {
  width: 100% !important;
  height: 100% !important;
  max-height: none !important;
  margin: 0 !important;
}

.dialog-header {
  display: flex;
  justify-content: space-between;
  align-items: center;
}

.dialog-fullscreen-button {
  color: #606266;
}

.canvas-container {
  display: flex;
  justify-content: center;
  align-items: center;
  height: 100%;
}

.centered-canvas {
  border: 1px solid black;
  width: 80%;
  height: 80%;
}

.slider-demo-block {
  max-width: 600px;
  display: flex;
  align-items: center;
}
</style>
